import sys
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import CommandBool  # 用于 arming 操作的服务类型
import time  # 用于控制重试之间的时间间隔（例如，等待 5 秒后重试）

# 自定义节点类；
class ArmClient(Node):
    def __init__(self):
        super().__init__('mavros_arm_client')  # 传入节点名称
        self.get_logger().info("客户端已创建！(arm)")

        # 创建与 MAVROS arming 服务的客户端，订阅'/mavros/cmd/arming' 服务
        self.client = self.create_client(CommandBool, '/mavros/cmd/arming')
        while not self.client.wait_for_service(timeout_sec=3.0):
            self.get_logger().info('Arming Service not available, waiting again...')
            time.sleep(3)  # 给服务一些时间准备

    def send_request(self, arm=True):
        request = CommandBool.Request()  # 创建请求消息
        request.value = arm  # 设置 arm 的状态，True 为 arm，False 为 disarm
        return self.client.call_async(request)

def main():
    # 初始化 ROS2 客户端；
    rclpy.init()
    arm_client = ArmClient()

    # 进行重试，直到服务成功
    success = False
    while not success:
        future = arm_client.send_request(arm=True)  # 请求 arm 操作
        rclpy.spin_until_future_complete(arm_client, future)

        if future.result() is not None and future.result().success:  # 检查是否成功 arm
            arm_client.get_logger().info('Arming successful!')
            success = True
        else:
            arm_client.get_logger().warn('Arming failed, retrying in 5 seconds...')
            time.sleep(5)  # 等待 5 秒后重试

    arm_client.get_logger().info('Drone armed successfully.')

    # 资源释放；
    arm_client.destroy_node()  # 销毁节点
    rclpy.shutdown()

if __name__ == '__main__':
    main()
